#ifndef SPHERE_HPP
#define SPHERE_HPP

#include <stdexcept>
#include <cmath>
#include <vector>
#include <iostream>
#include <cblas.h>
#include <lapacke.h>
#include "Function.hpp"
#include "Spline.hpp"
#include "Curvefitting.hpp"

std::vector<std::vector<double>> Sphere_Curve_Fitting(
    const std::vector<double>& x, 
    const std::vector<double>& y, 
    const std::vector<double>& z
) {
    // Check if input vectors are of the same size
    if (x.size() != y.size() || y.size() != z.size()) {
        throw std::invalid_argument("Input vectors must have the same size.");
    }

    int n = x.size();
    
    // Initialize required vectors with enough space
    std::vector<double> x_projected(n, 0.0);
    std::vector<double> y_projected(n, 0.0);
    
    double length = sqrt(x[0] * x[0] + y[0] * y[0] + z[0] * z[0]);
    
    // Project the points onto the 2D plane (using a simple projection for this example)
    for (int i = 0; i < n; ++i) {
        // Avoid division by zero: make sure (1 - z[i]) is not zero.
        if (length - z[i] == 0) {
            throw std::invalid_argument("z[i] cannot be 1 to avoid division by zero in projection.");
        }
        x_projected[i] = x[i] / (1 - z[i]/length)/length;
        y_projected[i] = y[i] / (1 - z[i]/length)/length;
    }
    
    // Create a PPForm spline for the projected 2D points
    PPForm spline(x_projected, y_projected, "natural");

    double min = x_projected[0];
    double max = x_projected[0];
    // Find min and max values of the projected x
    for (int i = 1; i < n; ++i) {
        if (x_projected[i] < min) {
            min = x_projected[i];
        }
        if (x_projected[i] > max) {
            max = x_projected[i];
        }
    }

    // Set the step size for spline interpolation
    double step = 0.1;
    int num_points = static_cast<int>((max - min) / step) + 1; // Calculate number of points
    std::vector<double> x_spline(num_points, 0.0);;
    std::vector<double> y_spline(num_points, 0.0);;
    std::vector<double> x_sphere_spline(num_points, 0.0);
    std::vector<double> y_sphere_spline(num_points, 0.0);
    std::vector<double> z_sphere_spline(num_points, 0.0);

    // Generate spline values
    int i = 0;
    for (double t = min; t <= max; t += step) {
        x_spline[i] = t;
        y_spline[i] = spline.getSplineValue(t);
        ++i;
    }

    // Normalize the spline points to lie on the unit sphere
    for (i = 0; i < num_points; ++i) {
        double length_spline = sqrt(x_spline[i] * x_spline[i] + y_spline[i] * y_spline[i] + 1);
        x_sphere_spline[i] = x_spline[i] / length_spline;
        y_sphere_spline[i] = y_spline[i] / length_spline;
        z_sphere_spline[i] = 1 / length_spline;

        // Re-scale to the original radius
        length_spline = sqrt(x_sphere_spline[i] * x_sphere_spline[i] + y_sphere_spline[i] * y_sphere_spline[i] + z_sphere_spline[i] * z_sphere_spline[i]);
        x_sphere_spline[i] *= length / length_spline;
        y_sphere_spline[i] *= length / length_spline;
        z_sphere_spline[i] *= length / length_spline;
    }

    // Store the three vectors (x, y, z coordinates) into a 2D vector and return it
    std::vector<std::vector<double>> result = {x_sphere_spline, y_sphere_spline, z_sphere_spline};
    return result;
}

#endif
